#ifndef _SERIPORT_H
#define _SERIPORT_H
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <unistd.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <errno.h>
#include <sys/stat.h>
#include <errno.h>
#include <iostream>
#include <assert.h>
#include <math.h>
#include <fstream>
#include <iomanip>

#define buffLen 1024
#define rcvTimeOut 2

#endif

using namespace std;

const double PI 	= 	3.1415926;
const double GRAVITY 	= 	9.8015;

using namespace std;
int hex_char_value(char c)
{
    if(c >= '0' && c <= '9')
        return c - '0';
    else if(c >= 'a' && c <= 'f')
        return (c - 'a' + 10);
    else if(c >= 'A' && c <= 'F')
        return (c - 'A' + 10);
    assert(0);
    return 0;
}

int hex_to_decimal(const char* szHex, int len)
{
    int result = 0;
    for(int i = 0; i < len; i++)
    {
        result += (int)pow((float)16, (int)len-i-1) * hex_char_value(szHex[i]);
    }
    return result;
}

void delay(int ms)
{
    while(ms--)
    {
        int i=10000;
        while(i--);
    }
}

/* Open the serial port */
int openPort(int fd, int comport)
{

    if (comport == 1)
    {
        fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
        if (-1 == fd)
        {
            perror("Can't Open Serial Port");
            return(-1);
        }
        else
        {
            printf("open ttyS0 .....\n");
        }
    }
    else if (comport == 2)
    {
        fd = open("/dev/ttyS1", O_RDWR | O_NOCTTY | O_NDELAY);
        if (-1 == fd)
        {
            perror("Can't Open Serial Port");
            return(-1);
        }
        else
        {
            printf("open ttyS1 .....\n");
        }
    }
    else if (comport == 3)
    {
        fd = open("/dev/ttyS2", O_RDWR | O_NOCTTY | O_NDELAY);
        if (-1 == fd)
        {
            perror("Can't Open Serial Port");
            return(-1);
        }
        else
        {
            printf("open ttyS2 .....\n");
        }
    }
    /*************************************************/
    else if (comport == 4)
    {
        fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
        if (-1 == fd)
        {
            perror("Can't Open Serial Port");
            return(-1);
        }
        else
        {
            printf("open ttyUSB1 .....\n");
        }
    }

    if (fcntl(fd, F_SETFL, 0)<0)
    {
        printf("fcntl failed!\n");
    }
    else
    {
        printf("fcntl=%d\n", fcntl(fd, F_SETFL, 0));
    }
    if (isatty(STDIN_FILENO) == 0)
    {
        printf("standard input is not a terminal device\n");
    }
    else
    {
        printf("is a tty success!\n");
    }
    printf("fd-open=%d\n", fd);
    return fd;
}

int setOpt(int fd, int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newtio, oldtio;
    if (tcgetattr(fd, &oldtio) != 0)
    {
        perror("SetupSerial 1");
        return -1;
    }
    bzero(&newtio, sizeof(newtio));
    newtio.c_cflag |= CLOCAL | CREAD;
    newtio.c_cflag &= ~CSIZE;

    switch (nBits)
    {
        case 7:
            newtio.c_cflag |= CS7;
            break;
        case 8:
            newtio.c_cflag |= CS8;
            break;
    }

    switch (nEvent)
    {
        case 'O':                     //奇校验
            newtio.c_cflag |= PARENB;
            newtio.c_cflag |= PARODD;
            newtio.c_iflag |= (INPCK | ISTRIP);
            break;
        case 'E':                     //偶校验
            newtio.c_iflag |= (INPCK | ISTRIP);
            newtio.c_cflag |= PARENB;
            newtio.c_cflag &= ~PARODD;
            break;
        case 'N':                    //无校验
            newtio.c_cflag &= ~PARENB;
            break;
    }

    switch (nSpeed)
    {
        case 2400:
            cfsetispeed(&newtio, B2400);
            cfsetospeed(&newtio, B2400);
            break;
        case 4800:
            cfsetispeed(&newtio, B4800);
            cfsetospeed(&newtio, B4800);
            break;
        case 9600:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
        case 115200:
            cfsetispeed(&newtio, B115200);
            cfsetospeed(&newtio, B115200);
            break;
        default:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
    }
    if (nStop == 1)
    {
        newtio.c_cflag &= ~CSTOPB;
    }
    else if (nStop == 2)
    {
        newtio.c_cflag |= CSTOPB;
    }
    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 0;
    tcflush(fd, TCIFLUSH);
    if ((tcsetattr(fd, TCSANOW, &newtio)) != 0)
    {
        perror("com set error");
        return -1;
    }
    printf("set done!\n");
    return 0;
}

int readDataTty(int fd, char *rcv_buf, int TimeOut, int Len)
{
    int retval;
    fd_set rfds;
    struct timeval tv;
    int ret, pos;
    tv.tv_sec = TimeOut / 1000;  //set the rcv wait time
    tv.tv_usec = TimeOut % 1000 * 1000;  //100000us = 0.1s

    pos = 0;
    while (1)
    {
        FD_ZERO(&rfds);
        FD_SET(fd, &rfds);
        retval = select(fd + 1, &rfds, NULL, NULL, &tv);
        if (retval == -1)
        {
            perror("select()");
            break;
        }
        else if (retval)
        {
            // tcflush(fd,TCIFLUSH);
            ret = read(fd, rcv_buf + pos, 1);
            if (-1 == ret)
            {
                break;
            }

            pos++;
            if (Len <= pos)
            {
                break;
            }
        }
        else
        {
            break;
        }
    }

    return pos;
}

/* Serial port send Data */
int sendDataTty(int fd, char *send_buf, int Len)
{
    ssize_t ret;

    ret = write(fd, send_buf, Len);

    if (ret == -1)
    {
        printf("write device error\n");
        return -1;
    }

    //    delay(100);
    return 1;
}

/* This function is used to initialize serial port.The return is very important.Sending and receiving data can not lack of it */
int seriportInit(void)
{
    int iSetOpt = 0;
    int fdSerial = 0;

    if((fdSerial = openPort(fdSerial, 4))<0)
    {
        perror("open_port error");
        return -1;
    }
    if((iSetOpt = setOpt(fdSerial,115200,8,'N',1))<0)
    {
        perror("set_opt error");
        return -1;
    }
    printf("Serial fdSerial=%d\n",fdSerial);
    tcflush(fdSerial,TCIOFLUSH);
    fcntl(fdSerial,F_SETFL,0);
    return fdSerial;
}

/*BNO080文档对串口描述*/
/*
The UART operates at 115200 b/s, 8 data bits, 1 stop bit and no parity.
Message: 0xAA AA DE 01 00 92 FF 25 08 8D FE EC FF D1 03 00 00 00 E7
Where:
Index = 0xDE = 222
Yaw = 00.01 ̊ (1 = 0x0001)
Pitch = -1.10 ̊ (-110 = 0xFF92)
Roll = 20.85 ̊ (2085 = 0x0825)
X-acceleration = -371 mg = -3.638 m/s 2 (-371 = 0xFE8D)
Y-acceleration = -20 mg = -0.196 m/s 2 (-20 = 0xFFEC)
Z-acceleration = 977 mg = -9.581 m/s 2 (977 = 0x03D1)
Checksum = 0xE7
*/
/* 115200 b/s, 8 data bits, 1 stop bit and no parity */

int main(int argc, char**argv)
{
    int count = 0;

    char rcv_buf[1024];

    int fdSerial = seriportInit();

    while (1)
    {
        // read
        readDataTty(fdSerial,rcv_buf,2,1024);
        short int accel_x_temp,accel_y_temp,accel_z_temp;
        short int gyro_x_temp, gyro_y_temp, gyro_z_temp;
        double accel_x_real,accel_y_real,accel_z_real;
        double gyro_x_real,gyro_y_real,gyro_z_real;

        if((rcv_buf[0] & 0XFF) == 0XAA &&(rcv_buf[1] & 0XFF) == 0XAA) {
            accel_x_temp = (rcv_buf[3]&0xFF) | (rcv_buf[4] << 8);
            accel_y_temp = (rcv_buf[5]&0xFF) | (rcv_buf[6] << 8);
            accel_z_temp = (rcv_buf[7]&0xFF) | (rcv_buf[8] << 8);
            gyro_x_temp = (rcv_buf[9]&0xFF)  | (rcv_buf[10]  << 8);
            gyro_y_temp = (rcv_buf[11]&0xFF) | (rcv_buf[12] << 8);
            gyro_z_temp = (rcv_buf[13]&0xFF) | (rcv_buf[14] << 8);

            accel_x_real = (double)(accel_x_temp) * 12 / 0x10000 * GRAVITY;
            accel_y_real = (double)(accel_y_temp) * 12 / 0x10000 * GRAVITY;
            accel_z_real = (double)(accel_z_temp) * 12 / 0x10000 * GRAVITY;
            gyro_x_real =  (double)(gyro_x_temp) * 2000 / 0x10000 * PI /180;
            gyro_y_real =  (double)(gyro_y_temp) * 2000 / 0x10000 * PI /180;
            gyro_z_real =  (double)(gyro_z_temp) * 2000 / 0x10000 * PI /180;

            accel_x_real = (double)(accel_x_temp) * 0.01;
            accel_y_real = (double)(accel_y_temp) * 0.01;
            accel_z_real = (double)(accel_z_temp) * 0.01;
            gyro_x_real = (double)(gyro_x_temp) * 0.001;
            gyro_y_real = (double)(gyro_y_temp) * 0.001;
            gyro_z_real = (double)(gyro_z_temp) * 0.001;

            cout << left;
            cout << showpos;
            cout << "yaw:" << fixed << setprecision(3) << setw(7) << accel_x_real << " ";
            cout << "pitch:" << fixed << setprecision(3) << setw(7) << accel_y_real << " ";
            cout << "roll:" << fixed << setprecision(3) << setw(7) << accel_z_real << " ";
            cout << "accx:" << fixed << setprecision(3) << setw(7) << gyro_x_real << " ";
            cout << "accy:" << fixed << setprecision(3) << setw(7) << gyro_y_real << " ";
            cout << "accz:" << fixed << setprecision(3) << setw(7) << gyro_z_real << flush << "\r";
        }
        delay(500);
    }

    return 0;
}
